import java.awt.Point;
import java.awt.geom.Point2D;
import java.util.Arrays;
import java.util.LinkedList;
import java.util.List;
import java.util.concurrent.LinkedBlockingQueue;
import java.util.concurrent.atomic.AtomicBoolean;
import java.util.concurrent.atomic.AtomicInteger;
import java.util.concurrent.locks.ReentrantLock;

/**
 * An implementation of the Chartographer interface. This module is responsible
 * for maintaining an internal map representation which is built from real world
 * sensor data retrieved from the robot. This specific implementation also
 * implements the Runnable interface as to allow for threaded map updates.
 * 
 * @author Emanuel Yifter Lindgren (dv11elr), Anders Lumio (dv11alo)
 *
 */
public class HIMMChartographer implements Chartographer, Runnable {
    private final int[][] internalMap;
    private final LinkedBlockingQueue<MapUpdate> workQueue;
    private static int INTERNAL_MAP_SCALER = 4;
    private static int HIMM_MAX_VAL = 15;
    private static int HIMM_MIN_VAL = 0;
    private static int HIMM_INCREMENT_VAL = 3;
    private static int HIMM_DECREMENT_VAL = 1;
    private static int LASER_DISTANCE_LIMIT = 10;
    private final double[] laserAngles;
    private final int x1, y1, x2, y2, internalXAxis, internalYAxis;
    private final Point refCornerMax;
    private final AtomicInteger rXInternal = new AtomicInteger();
    private final AtomicInteger rYInternal = new AtomicInteger();
    private final AtomicBoolean initialized = new AtomicBoolean(false);
    private final ShowMap mapOgrapher;
    private final ReentrantLock mapLock = new ReentrantLock();

    public HIMMChartographer(final int x1, final int y1, final int x2,
	    final int y2, final double[] laserAngles, final double robotX,
	    final double robotY) {

	// create the internal map based on x1,y1 lowerleft corner of map, and
	// x2,y2 upperright corner of map
	this.x1 = x1;
	this.y1 = y1;
	this.x2 = x2;
	this.y2 = y2;
	refCornerMax = new Point(Math.max(x1, x2), Math.max(y1, y2));

	this.laserAngles = laserAngles;
	internalXAxis = (int) Math.floor(Math.abs(x2 - x1))
	        * INTERNAL_MAP_SCALER;
	internalYAxis = (int) Math.floor(Math.abs(y2 - y1))
	        * INTERNAL_MAP_SCALER;

	internalMap = new int[internalXAxis][internalYAxis];
	workQueue = new LinkedBlockingQueue<MapUpdate>();
	mapOgrapher = new ShowMap(internalXAxis, internalYAxis);
	for (int x = 0; x < internalXAxis; x++) {
	    Arrays.fill(internalMap[x], 4);
	}

	rXInternal.set(bounceXToInternal(robotX));
	rYInternal.set(bounceYToInternal(robotY));

    }

    /*
     * (non-Javadoc)
     * 
     * @see Chartographer#updateMap(MapUpdate)
     */
    @Override
    public void updateMap(final MapUpdate update) {
	workQueue.offer(update);
    }

    /*
     * (non-Javadoc)
     * 
     * @see Chartographer#getMap()
     */
    @Override
    public int[][] getMap() {
	final int[][] internalMapCopy = new int[internalXAxis][internalYAxis];

	mapLock.lock();
	for (int x = 0; x < internalXAxis; x++) {
	    for (int y = 0; y < internalYAxis; y++) {
		internalMapCopy[x][y] = internalMap[x][y];
	    }
	}
	mapLock.unlock();
	return internalMapCopy;
    }

    @Override
    public void run() {
	try {
	    while (true) {
		/*
	         * The update queue is fed by the navigators update thread for
	         * continuous updates while the robot is driving.
	         */
		final MapUpdate update = workQueue.take();

		if (insideRealWorldMap(update.getRobotPos()[1],
		        update.getRobotPos()[0])) {

		    rXInternal.set(bounceXToInternal(update.getRobotPos()[1]));
		    rYInternal.set(bounceYToInternal(update.getRobotPos()[0]));

		    for (int i = 0; i < update.getEchoes().length; i++) {
			final double echoDistance = update.getEchoes()[i];

			final double echoAngle = laserAngles[i];
			final double errorAngle = update.getRobotAngle()
			        + echoAngle;

			final double bounceY = update.getRobotPos()[0]
			        + (Math.cos(errorAngle) * echoDistance);
			final double bounceX = update.getRobotPos()[1]
			        + (Math.sin(errorAngle) * echoDistance);

			if (insideRealWorldMap(bounceX, bounceY)
			        && (echoDistance < LASER_DISTANCE_LIMIT)) {
			    /*
		             * If a laser echoed within our laser distance we
		             * calculate the coordinates of the bounce and mark
		             * it as an obstacle, any points between the robot
		             * and the object are marked as empty.
		             */
			    final int x = bounceXToInternal(bounceX);
			    final int y = bounceYToInternal(bounceY);

			    final List<Point> linealong = BresenhamAlgorithm
				    .getLine(rXInternal.get(),
				            rYInternal.get(), x, y);
			    mapLock.lock();
			    for (int k = 0; k < linealong.size() - 1; k++) {
				final Point pos = linealong.get(k);
				if (internalMap[pos.x][pos.y] > HIMM_MIN_VAL) {
				    internalMap[pos.x][pos.y] -= HIMM_DECREMENT_VAL;
				}
			    }
			    // we don't write too close to robot
			    final double distanceToPoint = Math.sqrt(Math.pow(
				    rXInternal.get() - x, 2)
				    + Math.pow(rYInternal.get() - y, 2));
			    if (distanceToPoint > 0.6) {
				if ((internalMap[x][y] + HIMM_INCREMENT_VAL) < HIMM_MAX_VAL) {
				    internalMap[x][y] += HIMM_INCREMENT_VAL;
				} else {
				    internalMap[x][y] += HIMM_MAX_VAL
					    - internalMap[x][y];
				}
			    }
			    mapLock.unlock();

			} else {
			    /*
		             * We can clear any points on the map within our
		             * laser radius if they bounced somewhere outside of
		             * it.
		             */
			    final int x = echoToInternalX(bounceX);
			    final int y = echoToInternalY(bounceY);

			    final List<Point> linealong = BresenhamAlgorithm
				    .getLine(rXInternal.get(),
				            rYInternal.get(), x, y);

			    for (int k = 0; k < linealong.size(); k++) {
				final Point pos = linealong.get(k);

				final double distanceToPoint = Math
				        .sqrt(Math.pow(
				                rXInternal.get() - pos.x, 2)
				                + Math.pow(rYInternal.get()
				                        - pos.y, 2));

				if (outsideOfInternalMap(pos.x, pos.y)) {
				    break;
				}
				if (internalMap[pos.x][pos.y] > HIMM_MIN_VAL
				        && (distanceToPoint < LASER_DISTANCE_LIMIT
				                * INTERNAL_MAP_SCALER)
				        && internalMap[pos.x][pos.y] != HIMM_MAX_VAL) {
				    mapLock.lock();
				    internalMap[pos.x][pos.y] -= HIMM_DECREMENT_VAL;
				    mapLock.unlock();
				}
			    }
			}
		    }
		    if (insideRealWorldMap(update.getRobotPos()[1],
			    update.getRobotPos()[0])) {
			mapOgrapher.updateMap(internalMap, HIMM_MAX_VAL,
			        rXInternal.get(), rYInternal.get());
		    }
		}
		initialized.compareAndSet(false, true);
	    }
	} catch (final InterruptedException e) {
	    e.printStackTrace();
	}

    }

    /**
     * Helper method to determine whether a given coordinate is outside of the
     * internal map representation.
     * 
     * @param x
     *            the x-axis coordinate.
     * @param y
     *            the y-axis coordinate.
     * @return true if outside, false otherwise.
     */
    private boolean outsideOfInternalMap(final int x, final int y) {
	return (x < 0 || x > internalXAxis - 1)
	        || (y < 0 || y > internalYAxis - 1);
    }

    /**
     * Helper method to determine whether a given coordinate is within the real
     * world map we're mapping. Used to check if a laser bounce is outside our
     * map area.
     * 
     * @param bounceX
     *            the real world x-coordinate
     * @param bounceY
     *            the real world y-coordinate
     * @return true if inside, false otherwise.
     */
    private boolean insideRealWorldMap(final double bounceX,
	    final double bounceY) {
	final double xHighBoundary = Math.max(x1, x2);
	final double xLowBoundary = Math.min(x1, x2);
	final double yHighBoundary = Math.max(y1, y2);
	final double yLowBoundary = Math.min(y1, y2);
	return (bounceX > xLowBoundary && bounceX < xHighBoundary)
	        && (bounceY > yLowBoundary && bounceY < yHighBoundary);

    }

    /**
     * Helper method to convert a real world X-axis coordinate into the correct
     * internal X-axis coordinate. The bounces are not required to be within the
     * internal map which differentiate it from the bounceYToInternal method. It
     * is used for clearing coordinates that did not bounce on obstacles in the
     * robots laser radius.
     * 
     * @param bounceX
     *            the real world X-Axis coordinate.
     * @return the internal x-axis coordinate
     */
    private int echoToInternalX(final double bounceX) {
	final int x = (int) Math
	        .floor(((refCornerMax.x - bounceX) * INTERNAL_MAP_SCALER));
	return x;
    }

    /**
     * Helper method to convert a real world Y-axis coordinate into the correct
     * internal Y-axis coordinate. The bounces are not required to be within the
     * internal map which differentiate it from the bounceYToInternal method. It
     * is used for clearing coordinates that did not bounce on obstacles in the
     * robots laser radius.
     * 
     * @param bounceY
     *            the real world Y-Axis coordinate.
     * @return the internal y-axis coordinate
     */
    private int echoToInternalY(final double bounceY) {
	final int y = (int) Math
	        .floor(((refCornerMax.y - bounceY) * INTERNAL_MAP_SCALER));
	return y;
    }

    /**
     * Helper method to convert a real world X-axis coordinate into the correct
     * internal X-axis coordinate. Used to map laser bounces to the internal
     * map.
     * 
     * @param bounceX
     *            the real world X-Axis coordinate.
     * @return the internal x-axis coordinate
     */
    private int bounceXToInternal(final double bounceX) {
	final int x = (int) Math.floor(Math.abs(bounceX - refCornerMax.x)
	        * INTERNAL_MAP_SCALER);

	return x;

    }

    /**
     * Helper method to convert a real world Y-axis coordinate into the correct
     * internal Y-axis coordinate. Used to map laser bounces to the internal
     * map.
     * 
     * @param bounceY
     *            the real world Y-Axis coordinate.
     * @return the internal y-axis coordinate
     */
    private int bounceYToInternal(final double bounceY) {
	final int y = (int) Math.floor(Math.abs(bounceY - refCornerMax.y)
	        * INTERNAL_MAP_SCALER);

	return y;
    }

    /**
     * Helper method to convert an internal X-axis coordinate to real world
     * X-axis coordinate.
     * 
     * @param x
     *            internal x-coordinate
     * @return real world x coordinate
     */
    private double internalXToRealWorldX(final int x) {

	final double incrementX = 1.0 / INTERNAL_MAP_SCALER;
	double k = -1.0;

	final double middleAdjustment = (incrementX) / 2.0;

	final double newPos = (k * ((x / (double) INTERNAL_MAP_SCALER) + middleAdjustment))
	        + refCornerMax.x;
	return newPos;
    }

    /**
     * Helper method to convert an internal Y-axis coordinate to real world
     * Y-axis coordinate.
     * 
     * @param y
     *            internal y-coordinate
     * @return real world y coordinate
     */
    private double internalYToRealWorldY(final int y) {
	final double incrementY = 1.0 / INTERNAL_MAP_SCALER;
	final double middleAdjustment = (incrementY) / 2.0;
	double k = -1.0;

	final double newPos = (k * ((y / (double) INTERNAL_MAP_SCALER) + middleAdjustment))
	        + refCornerMax.y;
	return newPos;
    }

    /*
     * (non-Javadoc)
     * 
     * @see Chartographer#getRobotInternalPosition()
     */
    @Override
    public int[] getRobotInternalPosition() {

	final int[] ret = new int[] { rXInternal.get(), rYInternal.get() };

	return ret;
    }

    /*
     * (non-Javadoc)
     * 
     * @see Chartographer#convertInternalToRealWorld(java.util.List)
     */
    @Override
    public List<Point2D.Double> convertInternalToRealWorld(
	    final List<Point> internalCoords) {
	final LinkedList<Point2D.Double> ret = new LinkedList<Point2D.Double>();
	if (internalCoords == null) {
	    return ret;
	}
	for (final Point pos : internalCoords) {
	    ret.add(new Point2D.Double(internalXToRealWorldX(pos.x),
		    internalYToRealWorldY(pos.y)));
	}

	return ret;
    }

    /**
     * Tells whether the map has been updated at least once with sensor data.
     * 
     * @return true if it has been updated, false otherwise.
     */
    public boolean isInitialized() {
	return initialized.get();
    }

    @Override
    public int getInternalMapScale() {
	return INTERNAL_MAP_SCALER;
    }
}
